#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/nonfree/nonfree.hpp"
#include "opencv2/imgproc/imgproc_c.h"
#include "opencv2/legacy/legacy.hpp"
#include "opencv2/legacy/compat.hpp"

#include <iostream>
#include <vector>
#include <math.h>

using namespace std;
 
typedef struct rect
{
  int left ;
  int  top ;
  int right ;
  int bottom ; 
}rect ;


/* Normalize the Vector */
float* normalize_vector(int *input_vector)
{
  
  /* L2 normalization */
  float sq_sum = 0;
  int i ;
  float *norm_vector = (float*)malloc(32*sizeof(float));
  float sum_sq_root ;
  float theta ;
  float *final_norm_vector = (float*)malloc(32*sizeof(float));

  for(i=0;i<32;i++)
  {
     sq_sum += input_vector[i]*input_vector[i] ;  
  }

  /* 1 is detla E added to avoid divide by ZERO */  
  sq_sum += 1 ;
  sum_sq_root = sqrt(sq_sum); 
  for(i=0;i<32;i++)
  {
     norm_vector[i] = (float)(input_vector[i]/sum_sq_root) ;
     theta = 2/sqrt(32) ;
     if(norm_vector[i] > theta)
        norm_vector[i] = theta ;
     else if(norm_vector[i] < -theta)
        norm_vector[i] = -theta ;    
  } 
  
  sq_sum = 0 ;

   for(i=0;i<32;i++)
  {
     sq_sum += norm_vector[i]*norm_vector[i] ;  
  }
  
  sq_sum += 1 ;
  sum_sq_root = sqrt(sq_sum); 

  for(i=0;i<32;i++)
     final_norm_vector[i] = norm_vector[i]/sum_sq_root ;   
  
  return final_norm_vector ;
   
}

/* Find the 8 Dimensional Surf Descriptor for a Local Patch Window */
void surfdescriptor(rect patch , int **ImagePixelsArray2D,int *vec , int s_size )
{

  int i , j ;
  int x_size = (patch.right -patch.left + 1) ;
  int y_size = (patch.bottom - patch.top + 1) ;
  int DX[x_size][y_size] , DY[x_size][y_size] ;
  int tx = 0 , ty = 0 ;

  memset(vec,0,32*sizeof(int));
  
  for(i=0;i<x_size;i++)
  {
    for(j=0;j<y_size;j++)
  {
      
      if((patch.top + j) == (s_size-1))
      DX[i][j] = 0 ;
      else   
      DX[i][j] = ImagePixelsArray2D[patch.left+i][patch.top+j+1] - ImagePixelsArray2D[patch.left+i][patch.top+j] + ImagePixelsArray2D[patch.left+i+1][patch.top+j+1] - 
                 ImagePixelsArray2D[patch.left+i+1][patch.top+j] ;

      if((patch.left + i) == (s_size-1))
      DY[i][j] = 0 ;
      else
      DY[i][j] = ImagePixelsArray2D[patch.left+i+1][patch.top+j] - ImagePixelsArray2D[patch.left+i][patch.top+j] + ImagePixelsArray2D[patch.left+i+1][patch.top+j+1] - 
                 ImagePixelsArray2D[patch.left+i][patch.top+j+1] ;
      
      #ifdef DEBUG
      printf("i=%3d j=%3d dx=%3d dy=%3d %3d %3d %3d %3d\n",patch.left+i,patch.top+j,DX[i][j],DY[i][j], ImagePixelsArray2D[patch.left+i][patch.top+j+1],ImagePixelsArray2D[patch.left+i]
      [patch.top+j],ImagePixelsArray2D[patch.left+i+1][patch.top+j+1],ImagePixelsArray2D[patch.left+i+1][patch.top+j]);
      #endif  
  }
  }

  /* dx dy |dx| |dy|  with now taking into account sign of tx ty */
  /* 2 X 2 cells - 8X4 = 32 Cells */  

  /*         CELL 1       */
  
  for(i=0;i<(x_size)/2;i++)
    for(j=0;j<(y_size)/2;j++)
  {
   
  tx = DX[i][j] ;
  ty = DY[i][j] ;
  if( ty >= 0 )
  {  
     vec[0] += tx;
     vec[1] += abs(tx);
  } 
  else 
  {
     vec[2] += tx;
     vec[3] += abs(tx);
  }

  if ( tx >= 0 )
  {
     vec[4] += ty;
     vec[5] += abs(ty);
  }
  else
  {
     vec[6] += ty;
     vec[7] += abs(ty);
  }   
 
  }

  /*       CELL 2        */
  for(i=(x_size)/2;i<x_size;i++)
    for(j=0;j<(y_size)/2;j++)
  {
   
  tx = DX[i][j] ;
  ty = DY[i][j] ;
  if( ty >= 0 )
  {  
     vec[8] += tx;
     vec[9] += abs(tx);
  } 
  else 
  {
     vec[10] += tx;
     vec[11] += abs(tx);
  }

  if ( tx >= 0 )
  {
     vec[12] += ty;
     vec[13] += abs(ty);
  }
  else
  {
     vec[14] += ty;
     vec[15] += abs(ty);
  }   
 
  }

  /*      CELL 3         */
  for(i=0;i<(x_size)/2;i++)
    for(j=(y_size)/2;j<y_size;j++)
  {
   
  tx = DX[i][j] ;
  ty = DY[i][j] ;
  if( ty >= 0 )
  {  
     vec[16] += tx;
     vec[17] += abs(tx);
  } 
  else 
  {
     vec[18] += tx;
     vec[19] += abs(tx);
  }

  if ( tx >= 0 )
  {
     vec[20] += ty;
     vec[21] += abs(ty);
  }
  else
  {
     vec[22] += ty;
     vec[23] += abs(ty);
  }   
 
  }

  /*    CELL 4     */
  for(i=(x_size)/2;i<x_size;i++)
    for(j=(y_size)/2;j<y_size;j++)
  {
   
  tx = DX[i][j] ;
  ty = DY[i][j] ;
  if( ty >= 0 )
  {  
     vec[24] += tx;
     vec[25] += abs(tx);
  } 
  else 
  {
     vec[26] += tx;
     vec[27] += abs(tx);
  }

  if ( tx >= 0 )
  {
     vec[28] += ty;
     vec[29] += abs(ty);
  }
  else
  {
     vec[30] += ty;
     vec[31] += abs(ty);
  }   
 
  }

}

int main(int argc , char *argv[])
{

  int l , u ;
  int count = 0 ;
  int win_width = 40 ;
  int win_height = 40 ;

  vector<rect> rectangles ;
  
  rect temp ; 
 
  int subwin_width ;
  int subwin_height ;
  int subwin_inc_x ;
  int subwin_inc_y ;

  int step = 4 ;
  
  /* Local Patches */

  /* 1:1 */
  subwin_width = 16  ;
  subwin_height = 16 ;
  subwin_inc_x = 2 ;
  subwin_inc_y = 2 ;
  for(;subwin_width<=40,subwin_height<=40;subwin_width += subwin_inc_x ,subwin_height += subwin_inc_y)
  {

   for(u=0;u<=win_height-subwin_height;u=u+step)
    for(l=0;l<=win_width-subwin_width;l=l+step)
   {

       count++ ;
       temp.top = u ;
       temp.bottom = u+subwin_height-1 ; 
       temp.left = l ;
       temp.right = l+subwin_width-1 ;
       rectangles.push_back(temp);  
       #ifdef DEBUG
       printf("%d %d %d %d\n",l,u,l+subwin_width-1,u+subwin_height-1);
       #endif
   }
  }

  /* 1:2 */
  subwin_width = 16  ;
  subwin_height = 32 ;
  subwin_inc_x = 2 ;
  subwin_inc_y = 4 ;
  for(;subwin_width<=40,subwin_height<=40;subwin_width += subwin_inc_x ,subwin_height += subwin_inc_y)
  {

   for(u=0;u<=win_height-subwin_height;u=u+step)
    for(l=0;l<=win_width-subwin_width;l=l+step)
   {

       count++ ;
       temp.top = u ;
       temp.bottom = u+subwin_height-1 ; 
       temp.left = l ;
       temp.right = l+subwin_width-1 ;
       rectangles.push_back(temp);  
       #ifdef DEBUG
       printf("%d %d %d %d\n",l,u,l+subwin_width-1,u+subwin_height-1);
       #endif
 
   }
  }

  /* 2:1 */ 
  subwin_width = 32  ;
  subwin_height = 16 ;
  subwin_inc_x = 4 ;
  subwin_inc_y = 2 ;
  for(;subwin_width<=40,subwin_height<=40;subwin_width += subwin_inc_x ,subwin_height += subwin_inc_y)
  {

   for(u=0;u<=win_height-subwin_height;u=u+step)
    for(l=0;l<=win_width-subwin_width;l=l+step)
   {

       count++ ;
       temp.top = u ;
       temp.bottom = u+subwin_height-1 ; 
       temp.left = l ;
       temp.right = l+subwin_width-1 ;
       rectangles.push_back(temp);  
       #ifdef DEBUG
       printf("%d %d %d %d\n",l,u,l+subwin_width-1,u+subwin_height-1);
       #endif
 
   }
  }

  /* 2:3 */
  subwin_width = 16  ;
  subwin_height = 24 ;
  subwin_inc_x = 4 ;
  subwin_inc_y = 6 ;
  for(;subwin_width<=40,subwin_height<=40;subwin_width += subwin_inc_x ,subwin_height += subwin_inc_y)
  {

   for(u=0;u<=win_height-subwin_height;u=u+step)
    for(l=0;l<=win_width-subwin_width;l=l+step)
   {

       count++ ;
       temp.top = u ;
       temp.bottom = u+subwin_height-1 ; 
       temp.left = l ;
       temp.right = l+subwin_width-1 ;
       rectangles.push_back(temp);  
       #ifdef DEBUG
       printf("%d %d %d %d\n",l,u,l+subwin_width-1,u+subwin_height-1);
       #endif
 
   }
  }

  /* 3:2 */
  subwin_width = 24  ;
  subwin_height = 16 ;
  subwin_inc_x = 6 ;
  subwin_inc_y = 4 ;
  for(;subwin_width<=40,subwin_height<=40;subwin_width += subwin_inc_x ,subwin_height += subwin_inc_y)
  {

   for(u=0;u<=win_height-subwin_height;u=u+step)
    for(l=0;l<=win_width-subwin_width;l=l+step)
   {

       count++ ;
       temp.top = u ;
       temp.bottom = u+subwin_height-1 ; 
       temp.left = l ;
       temp.right = l+subwin_width-1 ;
       rectangles.push_back(temp);  

       #ifdef DEBUG
       printf("%d %d %d %d\n",l,u,l+subwin_width-1,u+subwin_height-1);
       #endif
 
   }
  }
  

  #ifdef DEBUG
  
  for(u=0;u<rectangles.size();u++)
  {
     printf("_________________________%d________________________ \n",u);
     printf("%d %d %d %d \n",rectangles[u].top,rectangles[u].bottom,rectangles[u].left,rectangles[u].right);
     printf("_________________________%d________________________ \n",u);  
  }
  #endif

  IplImage * inputImage ;

  inputImage = cvLoadImage(argv[1],CV_LOAD_IMAGE_GRAYSCALE);

  int channels = inputImage->nChannels ;
  int istep     = inputImage->widthStep ; 
  int height   = inputImage->height ;
  int width    = inputImage->width ;
  uchar *data     = (uchar *)inputImage->imageData ;
  int **ImagePixelsArray2D ;

  ImagePixelsArray2D = (int **)malloc(sizeof(int * )*(height + 1));
  
  for(u = 0; u < height+1; u++)
    {
        ImagePixelsArray2D[u] = (int *)malloc(sizeof(int)*(width + 1));
        for(l = 0; l < width+1; l++)
        {   
            if(( u == height )||( l == width ))
            {
             ImagePixelsArray2D[u][l] = 0 ;
            } 
            else   
             ImagePixelsArray2D[u][l] = data[u*istep + l*channels] ;
        }
    }


  #ifdef DEBUG
  printf("Got : 2D matrix for Image \n");
  printf("height = %d width=%d \n",height,width);
  for(u=0;u<height+1;u++)
  {
    for(l=0;l<width+1;l++)
      printf("%3d ",ImagePixelsArray2D[u][l]);
    printf("\n");
  }
  #endif

  int *tempSurfVector ;
  float *normSurfVector ;

  for(u=0;u<rectangles.size();u++)
  {  
     tempSurfVector = (int*)malloc(32*sizeof(int));
     normSurfVector = (float*)malloc(32*sizeof(float));
 
     surfdescriptor(rectangles[u],ImagePixelsArray2D,tempSurfVector,40) ;
     #ifdef DEBUG
     printf("________________________%d__________________________ \n",u);
     printf("%d %d %d %d \n",rectangles[u].top,rectangles[u].bottom,rectangles[u].left,rectangles[u].right);
     printf("________________________%d___________________________\n",u);     
     
     
     for(l=0;l<32;l++)
       printf("%4d ",tempSurfVector[l]);
     printf("\n");
     #endif  
     
     printf("-1 "); 
     normSurfVector = normalize_vector(tempSurfVector) ;
     for(l=0;l<32;l++)
       printf("%4f ",normSurfVector[l]);
     printf("\n");      
      
     free(tempSurfVector); 
     free(normSurfVector);  
  }
   
  return 0 ; 

}
